#!/usr/bin/python

import roslib; roslib.load_manifest('darci') 
import rospy
import sys, time, os
import math, numpy as np
import time
from geometry_msgs.msg import Wrench
from geometry_msgs.msg import Twist
from m3ctrl_msgs.msg import M3JointCmd
class Lead():
   
    def __init__(self):
        self.RIGHT_ARM = 0
        self.LEFT_ARM = 1
        self.JOINT_MODE_ROS_THETA_GC = 2
        self.SMOOTHING_MODE_SLEW = 1
        self.humanoid_pub = rospy.Publisher('/humanoid_command', M3JointCmd)
        rospy.sleep(1.0)
    def move_arm(self):
        joint_cmd_right = M3JointCmd()
        joint_cmd_left = M3JointCmd()
        joint_cmd_right.header.stamp = rospy.Time.now()
        joint_cmd_right.header.frame_id = 'humanoid_cmd_right'
        joint_cmd_left.header.stamp = rospy.Time.now()
        joint_cmd_left.header.frame_id = 'humanoid_cmd_left'
        
        joint_cmd_right.chain = [0,0,0,0,0,0,0]
        joint_cmd_right.chain_idx = np.arange(7,dtype=np.int16)
        joint_cmd_right.control_mode = [2,2,2,2,2,2,2]  
        joint_cmd_right.stiffness = np.array([1.,1.,1.,1.,1.,1.,1.],dtype=np.float32) 
        joint_cmd_right.velocity = np.array([1.,1.,1.,1.,1.,1.,1.],dtype=np.float32)
        joint_cmd_right.position = np.array([0.,0.,0.,1.57,0.,0.,0.],dtype=np.float32)
        joint_cmd_right.smoothing_mode = [1,1,1,1,1,1,1]
       
        joint_cmd_left.chain = [1,1,1,1,1,1,1]
        joint_cmd_left.chain_idx = np.arange(7,dtype=np.int16)
        joint_cmd_left.control_mode = [2,2,2,2,2,2,2]  
        joint_cmd_left.stiffness = np.array([1.,1.,1.,1.,1.,1.,1.],dtype=np.float32) 
        joint_cmd_left.velocity = np.array([1.,1.,1.,1.,1.,1.,1.],dtype=np.float32)
        joint_cmd_left.position = np.array([0.,0.,0.,1.57,0.,0.,0.],dtype=np.float32)
        joint_cmd_left.smoothing_mode = [1,1,1,1,1,1,1]
        self.humanoid_pub.publish(joint_cmd_right)
        rospy.sleep(1.0)
        self.humanoid_pub.publish(joint_cmd_left)
        rospy.sleep(1.0)

if __name__ == '__main__':
    
    rospy.init_node( 'move_arm_node', anonymous = True )
    leader = Lead()
    leader.move_arm()
    rospy.spin()

